
//#include "esp_system.h"
//#include "nvs_flash.h"
//#include "nvs.h"
#include "driver/gpio.h"
#include "soc/gpio_struct.h"
#include "soc/gpio_reg.h"
#include "btstack.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"


#define PSX_CLK CONFIG_HW_PSX_CLK
#define PSX_DAT CONFIG_HW_PSX_DAT
#define PSX_ATT CONFIG_HW_PSX_ATT
#define PSX_CMD CONFIG_HW_PSX_CMD 

#define DELAY() asm("nop; nop; nop; nop;nop; nop; nop; nop;nop; nop; nop; nop;nop; nop; nop; nop;")

// 引脚定义（根据实际接线修改）
#define PS2_ATT    GPIO_NUM_5   // Attention (主机→手柄，低电平有效)
#define PS2_CMD    GPIO_NUM_23  // Command (主机→手柄)
#define PS2_CLK    GPIO_NUM_18  // Clock (主机→手柄，下降沿有效)
#define PS2_DAT    GPIO_NUM_19  // Data (手柄→主机，开漏输出)

// 协议常量
#define PS2_CMD_READ_INPUT   0x42
#define PS2_CMD_ENABLE_RUMBLE 0x4D
#define TAG "PS2_EMULATOR"

// 全局状态变量
typedef struct {
    uint8_t buttons[3];    // 按钮状态（Byte0-2）
    uint8_t right_x;       // 右摇杆X
    uint8_t right_y;       // 右摇杆Y
    uint8_t left_x;        // 左摇杆X
    uint8_t left_y;        // 左摇杆Y
} ps2_state_t;

volatile ps2_state_t current_state = {
    .buttons = {0xFF, 0xFF, 0xFF}, // 默认无按键按下
    .right_x = 0x80,               // 摇杆居中
    .right_y = 0x80,
    .left_x = 0x80,
    .left_y = 0x80
};

// 初始化GPIO
void init_ps2_gpio() {
    // 配置输出引脚（ATT, CMD, CLK）
    gpio_config_t io_out = {
        .pin_bit_mask = (1ULL << PS2_ATT) | (1ULL << PS2_CMD) | (1ULL << PS2_CLK),
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_DISABLE
    };
    gpio_config(&io_out);

    // 配置输入引脚（DAT）
    gpio_config_t io_in = {
        .pin_bit_mask = (1ULL << PS2_DAT),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_ENABLE,
        .intr_type = GPIO_INTR_DISABLE
    };
    gpio_config(&io_in);

    // 初始状态
    gpio_set_level(PS2_ATT, 1);  // 空闲时ATT为高
    gpio_set_level(PS2_CLK, 1);  // 时钟空闲高电平
    gpio_set_level(PS2_CMD, 1);  // CMD初始高
}

// 精确延时（微秒级）
inline void ps2_delay_us(uint32_t us) {
    uint32_t ticks = us * (CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ / 5);
    volatile uint32_t count = 0;
    for(; count < ticks; count++);
}

// 发送一个字节（主机→手柄）
void send_byte_to_host(uint8_t data) {
    for(int i = 0; i < 8; i++) {
        gpio_set_level(PS2_CLK, 0);
        ps2_delay_us(10); // 时钟低电平保持时间

        // 在时钟下降沿设置DAT数据
        gpio_set_level(PS2_DAT, (data & (1 << i)) ? 1 : 0);
        ps2_delay_us(5);

        gpio_set_level(PS2_CLK, 1);
        ps2_delay_us(10); // 时钟高电平保持时间
    }
}

// 接收一个字节（手柄→主机）
uint8_t receive_byte_from_host() {
    uint8_t data = 0;
    for(int i = 0; i < 8; i++) {
        gpio_set_level(PS2_CLK, 0);
        ps2_delay_us(10);

        // 在时钟上升沿读取CMD数据
        if(gpio_get_level(PS2_CMD)) {
            data |= (1 << i);
        }

        gpio_set_level(PS2_CLK, 1);
        ps2_delay_us(10);
    }
    return data;
}

// 生成手柄响应数据
void generate_response(uint8_t *buffer) {
    // 标准响应头
    buffer[0] = 0x41; // Digital模式标识
    buffer[1] = 0x5A; // ID代码

    // 按钮状态（LOW有效）
    buffer[2] = current_state.buttons[0]; // SELECT:7, L3:6, R3:5, START:4, ↑:3, →:2, ↓:1, ←:0
    buffer[3] = current_state.buttons[1]; // L2:7, R2:6, L1:5, R1:4, △:3, ○:2, ×:1, □:0

    // 摇杆数据（0x00-0xFF）
    buffer[4] = current_state.right_x;
    buffer[5] = current_state.right_y;
    buffer[6] = current_state.left_x;
    buffer[7] = current_state.left_y;
}

// 处理主机命令
void process_host_command() {
    uint8_t cmd = receive_byte_from_host();
    uint8_t response[8] = {0};

    switch(cmd) {
        case PS2_CMD_READ_INPUT:
            generate_response(response);
            for(int i = 0; i < 8; i++) {
                send_byte_to_host(response[i]);
            }
            break;

        case PS2_CMD_ENABLE_RUMBLE:
            // 忽略震动马达命令
            receive_byte_from_host(); // 跳过参数字节
            send_byte_to_host(0xFA);  // 发送ACK
            break;

        default:
            ESP_LOGI(TAG, "Unknown command: 0x%02X", cmd);
            send_byte_to_host(0xFE); // 发送错误码
            break;
    }
}

// PS2模拟主任务
void ps2_emulator_task(void *pvParameter) {
    init_ps2_gpio();
    ESP_LOGI(TAG, "PS2 Emulator Started");

    while(1) {
        // 检测ATT信号（低电平激活）
        if(gpio_get_level(PS2_ATT) == 0) {
            process_host_command();
            
            // 等待ATT释放（防止重复处理）
            while(gpio_get_level(PS2_ATT) == 0) {
                vTaskDelay(1 / portTICK_PERIOD_MS);
            }
        }
        vTaskDelay(10 / portTICK_PERIOD_MS);
    }
}

void joystick_task() {
    // 示例：设置按钮状态（这里可以替换为实际输入检测）
    current_state.buttons[0] = 0xFE; // 模拟按下←键
    current_state.left_x = 0x00;     // 左摇杆向左

    xTaskCreate(ps2_emulator_task, "ps2_emu", 4096, NULL, 5, NULL);
}

/* Sends and receives a byte from/to the PSX controller using SPI */
static int psxSendRecv(int send) {
	int x;
	int ret=0;
	volatile int delay;
	
#if 0
	while(1) {
		GPIO.out_w1ts=(1<<PSX_CMD);
		GPIO.out_w1ts=(1<<PSX_CLK);
		GPIO.out_w1tc=(1<<PSX_CMD);
		GPIO.out_w1tc=(1<<PSX_CLK);
	}
#endif

	GPIO.out_w1tc=(1<<PSX_ATT);
	for (delay=0; delay<100; delay++);
	for (x=0; x<8; x++) {
		if (send&1) {
			GPIO.out_w1ts=(1<<PSX_CMD);
		} else {
			GPIO.out_w1tc=(1<<PSX_CMD);
		}
		DELAY();
		for (delay=0; delay<100; delay++);
		GPIO.out_w1tc=(1<<PSX_CLK);
		for (delay=0; delay<100; delay++);
		GPIO.out_w1ts=(1<<PSX_CLK);
		ret>>=1;
		send>>=1;
		if (GPIO.in&(1<<PSX_DAT)) ret|=128;
	}
	return ret;
}

static void psxDone() {
	DELAY();
	REG_WRITE(GPIO_OUT_W1TS_REG, (1<<PSX_ATT));
}


int psxReadInput() {

	int b1, b2;
	psxSendRecv(0x01); //wake up
	psxSendRecv(0x42); //get data
	psxSendRecv(0xff); //should return 0x5a
	b1=psxSendRecv(0xff); //buttons byte 1
	b2=psxSendRecv(0xff); //buttons byte 2
	psxDone();
	return (b2<<8)|b1;

}


void psxcontrollerInit() {
	volatile int delay;
	int t;
	gpio_config_t gpioconf[2]={
		{
			.pin_bit_mask=(1<<PSX_CLK)|(1<<PSX_CMD)|(1<<PSX_ATT), 
			.mode=GPIO_MODE_OUTPUT, 
			.pull_up_en=GPIO_PULLUP_DISABLE, 
			.pull_down_en=GPIO_PULLDOWN_DISABLE, 
			.intr_type=GPIO_INTR_DISABLE
		},{
			.pin_bit_mask=(1<<PSX_DAT), 
			.mode=GPIO_MODE_INPUT, 
			.pull_up_en=GPIO_PULLUP_ENABLE, 
			.pull_down_en=GPIO_PULLDOWN_DISABLE, 
			.intr_type=GPIO_INTR_DISABLE
		}
	};
	gpio_config(&gpioconf[0]);
	gpio_config(&gpioconf[1]);
	
	//Send a few dummy bytes to clean the pipes.
	psxSendRecv(0);
	psxDone();
	for (delay=0; delay<500; delay++) DELAY();
	psxSendRecv(0);
	psxDone();
	for (delay=0; delay<500; delay++) DELAY();
	//Try and detect the type of controller, so we can give the user some diagnostics.
	psxSendRecv(0x01);
	t=psxSendRecv(0x00);
	psxDone();
	if (t==0 || t==0xff) {
		printf("No PSX/PS2 controller detected (0x%X). You will not be able to control the game.\n", t);
	} else {
		printf("PSX controller type 0x%X\n", t);
	}
}

void JoystickWorker(){
    psxcontrollerInit();
    
}



static uint16_t  rfcomm_mtu;
static uint16_t  rfcomm_cid = 0;

// set mode counter
static uint8_t mode = 0;

volatile uint16_t input = 0;

//write msg to char[5]
static void MogaSendMsg(int code,char* msg){
    int SENDMSG_LEN = 5;
    int m_CID = 5;
    msg[0]= 0x5a;
    msg[1]=SENDMSG_LEN;
    msg[2]=code;
    msg[3]=m_CID;
    int chksum = 0;
    for(int i=0;i<SENDMSG_LEN-1;i++)
        chksum = msg[i]^chksum;
    msg[4]= chksum;
}

static void MogaSetMode(int rfcomm_channel_id){
    char msg[5];
    if(mode == 0){
        MogaSendMsg(67, msg);
        rfcomm_send(rfcomm_channel_id, (uint8_t*) msg, 5); 
        printf("send%d:%s\n",mode, (char*)msg);
        rfcomm_request_can_send_now_event(rfcomm_channel_id);
        mode +=1;
    }
    else if(mode == 1){
        //listen mode
        MogaSendMsg(69, msg);
        rfcomm_send(rfcomm_channel_id, (uint8_t*) msg, 5);
        printf("send%d:%s\n",mode, (char*)msg);
        rfcomm_request_can_send_now_event(rfcomm_channel_id);
        mode +=1;
    }
    else if(mode == 2){    
        MogaSendMsg(70, msg);
        rfcomm_send(rfcomm_channel_id, (uint8_t*) msg, 5); 
        printf("send%d:%s\n",mode, (char*)msg);
    }
}
static uint16_t osd_getjoyinput(){
    return input;
}
static uint16_t osd_setinput(uint8_t *packet)
{

        input=0; //uint16_t 
        input |= (1<<5)&packet[4] ; //select
        //0
        //0
        input |= (1<<4)&packet[4] ; //start
        input |= packet[5]&1 ; //up
        input |= (1<<3)&packet[5] ; //right
        input |= (1<<1)&packet[5] ; //down
        input |= (1<<2)&packet[5] ; //left
        //0
        //0
        //0
        //0
        //0 //reset
        input |= (1<<2)&packet[4]; //a
        input |= (1<<1)&packet[4]; //b
        //0 //reset

        return input;
}
   
 
static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
 
static btstack_packet_callback_registration_t hci_event_callback_registration;
 
static void packet_handler (uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
 
   switch(packet_type){
    case HCI_EVENT_PACKET:
        switch(hci_event_packet_get_type(packet)){
            case RFCOMM_EVENT_CHANNEL_OPENED:
                if (rfcomm_event_channel_opened_get_status(packet)) {
                    printf("RFCOMM channel open failed, status %u\n", rfcomm_event_channel_opened_get_status(packet));
                }
                else{
                    printf("the channel created!\n");
                    rfcomm_cid = rfcomm_event_channel_opened_get_rfcomm_cid(packet);
                    rfcomm_mtu = rfcomm_event_channel_opened_get_max_frame_size(packet);
                    printf("RFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u\n", rfcomm_cid, rfcomm_mtu);
                    printf("MogaSetMode have rfcomm_request before !\n");
                    rfcomm_request_can_send_now_event(rfcomm_cid);
                }               
                break;
            case RFCOMM_EVENT_CHANNEL_CLOSED:
                printf("the channel closed! \n");
                rfcomm_cid = 0;
                break;

            case RFCOMM_EVENT_CAN_SEND_NOW:
                printf("we can send packet now! \n");
                MogaSetMode(rfcomm_cid);
                break;

        }
        break;
    
    case RFCOMM_DATA_PACKET:
        //handle RFCOMM data packets
        //printf("received packet here now!\n");
        printf("RCV: '");    
        printf("%x ",osd_setinput(packet));
        printf("'\n");
        break ;
    
    default:
        printf("default packet_type:%d\n",packet_type);

   }

   
   
   /*
    uint16_t rfcomm_channel_id;
 
   if (packet_type == HCI_EVENT_PACKET
       && hci_event_packet_get_type(packet) == RFCOMM_EVENT_INCOMING_CONNECTION){
 
       rfcomm_channel_id = rfcomm_event_incoming_connection_get_rfcomm_cid(packet);
       rfcomm_accept_connection(rfcomm_channel_id);
 
     }else if (packet_type == RFCOMM_DATA_PACKET){
 
        printf("Received data: '");
 
        for (int i=0;i<size;i++){
             putchar(packet[i]);
         }
 
          printf("\n----------------\n");
    }
    */
}
 
int btstack_main(int argc, const char * argv[]){

    /*
    // 初始化 NVS
    esp_err_t err = nvs_flash_init();
    if (err == ESP_ERR_NVS_NO_FREE_PAGES ) {//|| err == ESP_ERR_NVS_NEW_VERSION_FOUND
        ESP_ERROR_CHECK(nvs_flash_erase());
        err = nvs_flash_init();
    }
    ESP_ERROR_CHECK(err);
    */

    uint16_t cid;
    bd_addr_t addr={0x00,0x1E,0xB5,0x0C,0x73,0xA6};


    xTaskCreate(ps2_emulator_task, "ps2_emu", 4096, NULL, 5, NULL);

    hci_event_callback_registration.callback = &packet_handler;
    hci_add_event_handler(&hci_event_callback_registration);
 
    l2cap_init();
 
    rfcomm_init();
 
    sdp_init();
 
    //gap_discoverable_control(1);

    hci_power_control(HCI_POWER_ON);

    rfcomm_create_channel(packet_handler,addr, 1, &cid); 

    return 0;
}

/*
void app_main() {
    
    // 创建SPI通信任务
    xTaskCreate(ps2_emulator_task, "ps2_emu", 4096, NULL, 5, NULL);
    
    ESP_LOGI(TAG, "PS2手柄模拟器已启动");
    
    // 主循环中可以更新手柄状态
    while(1) {
        // 示例：每5秒切换一次选择按钮状态
        static uint8_t select_state = 0;
        select_state = !select_state;
        
        
        vTaskDelay(5000 / portTICK_PERIOD_MS);
    }
}
*/